#include "Spi_Dev_X3.h"
#include "Utilities/Utilities.h"
#include "rclcpp/rclcpp.hpp"

static rclcpp::Logger TRACE = rclcpp::get_logger("Spi_Dev_X3");

Spi_Dev_X3::Spi_Dev_X3(const std::string& node)
    : its_spi_dev_fd(0), its_spi_node(node), its_speed(8000000/*max=50000000*/), its_delay(0), its_mode(0), its_bits(8), is_verbose(false)
{
	RCLCPP_INFO_STREAM(TRACE, "create m55 dev : " << its_spi_node);
}

bool Spi_Dev_X3::open_dev()
{
    its_spi_dev_fd = open(its_spi_node.c_str(), O_RDWR);
	RCLCPP_INFO_STREAM(TRACE, "spi fd : " << its_spi_dev_fd);

	    /*
	 * spi mode
	 */
	auto ret = ioctl(its_spi_dev_fd, SPI_IOC_WR_MODE32, &its_mode);
	if (ret == -1)
		return false;

	ret = ioctl(its_spi_dev_fd, SPI_IOC_RD_MODE32, &its_mode);
	if (ret == -1)
		return false;
	/*
	 * bits per word
	 */
	ret = ioctl(its_spi_dev_fd, SPI_IOC_WR_BITS_PER_WORD, &its_bits);
	if (ret == -1)
		return false;

	ret = ioctl(its_spi_dev_fd, SPI_IOC_RD_BITS_PER_WORD, &its_bits);
	if (ret == -1)
		return false;
	
	/*
	 * max speed hz
	 */
	ret = ioctl(its_spi_dev_fd, SPI_IOC_WR_MAX_SPEED_HZ, &its_speed);
	if (ret == -1)
		return false;

	ret = ioctl(its_spi_dev_fd, SPI_IOC_RD_MAX_SPEED_HZ, &its_speed);
	if (ret == -1)
		return false;

	RCLCPP_INFO_STREAM(TRACE,  "spi mode: 0x" << std::hex << its_mode);
	RCLCPP_INFO_STREAM(TRACE,  "bits per word 0x" << std::hex << its_bits );
	RCLCPP_INFO_STREAM(TRACE,  "max speed: " << its_speed << "Hz" );
	RCLCPP_INFO_STREAM(TRACE,  "spi node :" << its_spi_node );

	return its_spi_dev_fd > 0;
}

bool Spi_Dev_X3::init_dev(uint32_t speed, uint16_t mode, uint8_t bits, uint16_t delay, bool verbose)
{
    its_mode = mode;
    its_speed = speed;
    its_bits = bits;
    its_delay = delay;
	is_verbose = verbose;

    return true;
}

void Spi_Dev_X3::close_dev()
{
    close(its_spi_dev_fd);
    its_spi_dev_fd = 0;
}

bool Spi_Dev_X3::send_msg()
{
    return true;
}

bool Spi_Dev_X3::recv_msg()
{
    return true;
}

const std::string& Spi_Dev_X3::node_name() const
{
	return its_spi_node;
}

bool Spi_Dev_X3::transfer(uint8_t const *tx, uint8_t const *rx, size_t len)
{
    if(its_spi_dev_fd > 0)
    {   
	    int ret;
		// int out_fd;
		struct spi_ioc_transfer tr;
		tr.tx_buf = (unsigned long)tx;
		tr.rx_buf = (unsigned long)rx;
		tr.len = len;
		tr.delay_usecs = its_delay;
		tr.speed_hz = its_speed;
		tr.bits_per_word = its_bits;
		tr.tx_nbits = 0;	//must init to 0
		tr.rx_nbits = 0;	//must init to 0

		if (its_mode & SPI_TX_QUAD)
			tr.tx_nbits = 4;
		else if (its_mode & SPI_TX_DUAL)
			tr.tx_nbits = 2;
		if (its_mode & SPI_RX_QUAD)
			tr.rx_nbits = 4;
		else if (its_mode & SPI_RX_DUAL)
			tr.rx_nbits = 2;
		if (!(its_mode & SPI_LOOP)) {
			if (its_mode & (SPI_TX_QUAD | SPI_TX_DUAL))
				tr.rx_buf = 0;
			else if (its_mode & (SPI_RX_QUAD | SPI_RX_DUAL))
				tr.tx_buf = 0;
		}

		tr.tx_buf = (unsigned long)tx,
		tr.rx_buf = (unsigned long)rx,

		
        ret = ioctl(its_spi_dev_fd, SPI_IOC_MESSAGE(1), &tr);


		RCLCPP_INFO_STREAM(TRACE,  "len : " << len);
		RCLCPP_INFO_STREAM(TRACE,  "crc : " << static_cast<uint16_t>(tx[len-1]));
		RCLCPP_INFO_STREAM(TRACE,  "last data : " << static_cast<uint16_t>(tx[len-2]));

		if(is_verbose)
		{
			Utility::hex_dump(tx, len, (its_spi_node+":Tx").c_str());
			Utility::hex_dump(rx, len, (its_spi_node+":Rx").c_str());
		}

        return ret > 0;
    }
    else
    {
        RCLCPP_INFO_STREAM(TRACE,  "open dev first" );
        return false;
    }
}
